diff --git a/arch/arm/cpu/arm926ejs/mxs/spl_power_init.c b/arch/arm/cpu/arm926ejs/mxs/spl_power_init.c
index 4b917bd..0d80158 100644
--- a/arch/arm/cpu/arm926ejs/mxs/spl_power_init.c
+++ b/arch/arm/cpu/arm926ejs/mxs/spl_power_init.c
@@ -766,198 +766,112 @@
 	return 0;
 }
 
-void mxs_power_set_vddio(uint32_t new_target, uint32_t new_brownout)
+struct mxs_vddx_cfg {
+	uint32_t		*reg;
+	uint8_t			step_mV;
+	uint16_t		lowest_mV;
+	int			(*powered_by_linreg)(void);
+	uint32_t		trg_mask;
+	uint32_t		bo_irq;
+	uint32_t		bo_enirq;
+	uint32_t		bo_offset_mask;
+	uint32_t		bo_offset_offset;
+};
+
+const struct mxs_vddx_cfg mxs_vddio_cfg = {
+	.reg			= &(((struct mxs_power_regs *)MXS_POWER_BASE)->
+					hw_power_vddioctrl),
+	.step_mV		= 50,
+	.lowest_mV		= 2800,
+	.powered_by_linreg	= mxs_get_vddio_power_source_off,
+	.trg_mask		= POWER_VDDIOCTRL_TRG_MASK,
+	.bo_irq			= POWER_CTRL_VDDIO_BO_IRQ,
+	.bo_enirq		= POWER_CTRL_ENIRQ_VDDIO_BO,
+	.bo_offset_mask		= POWER_VDDIOCTRL_BO_OFFSET_MASK,
+	.bo_offset_offset	= POWER_VDDIOCTRL_BO_OFFSET_OFFSET,
+};
+
+const struct mxs_vddx_cfg mxs_vddd_cfg = {
+	.reg			= &(((struct mxs_power_regs *)MXS_POWER_BASE)->
+					hw_power_vdddctrl),
+	.step_mV		= 25,
+	.lowest_mV		= 800,
+	.powered_by_linreg	= mxs_get_vddd_power_source_off,
+	.trg_mask		= POWER_VDDDCTRL_TRG_MASK,
+	.bo_irq			= POWER_CTRL_VDDD_BO_IRQ,
+	.bo_enirq		= POWER_CTRL_ENIRQ_VDDD_BO,
+	.bo_offset_mask		= POWER_VDDDCTRL_BO_OFFSET_MASK,
+	.bo_offset_offset	= POWER_VDDDCTRL_BO_OFFSET_OFFSET,
+};
+
+static void mxs_power_set_vddx(const struct mxs_vddx_cfg *cfg,
+				uint32_t new_target, uint32_t new_brownout)
 {
 	struct mxs_power_regs *power_regs =
 		(struct mxs_power_regs *)MXS_POWER_BASE;
 	uint32_t cur_target, diff, bo_int = 0;
 	uint32_t powered_by_linreg = 0;
+	int adjust_up, tmp;
 
-	new_brownout = (new_target - new_brownout + 25) / 50;
+	new_brownout = DIV_ROUND(new_target - new_brownout, cfg->step_mV);
 
-	cur_target = readl(&power_regs->hw_power_vddioctrl);
-	cur_target &= POWER_VDDIOCTRL_TRG_MASK;
-	cur_target *= 50;	/* 50 mV step*/
-	cur_target += 2800;	/* 2800 mV lowest */
+	cur_target = readl(cfg->reg);
+	cur_target &= cfg->trg_mask;
+	cur_target *= cfg->step_mV;
+	cur_target += cfg->lowest_mV;
 
-	powered_by_linreg = mxs_get_vddio_power_source_off();
-	if (new_target > cur_target) {
+	adjust_up = new_target > cur_target;
+	powered_by_linreg = cfg->powered_by_linreg();
 
+	if (adjust_up) {
 		if (powered_by_linreg) {
-			bo_int = readl(&power_regs->hw_power_vddioctrl);
-			clrbits_le32(&power_regs->hw_power_vddioctrl,
-					POWER_CTRL_ENIRQ_VDDIO_BO);
+			bo_int = readl(cfg->reg);
+			clrbits_le32(cfg->reg, cfg->bo_enirq);
 		}
-
-		setbits_le32(&power_regs->hw_power_vddioctrl,
-				POWER_VDDIOCTRL_BO_OFFSET_MASK);
-		do {
-			if (new_target - cur_target > 100)
-				diff = cur_target + 100;
-			else
-				diff = new_target;
-
-			diff -= 2800;
-			diff /= 50;
-
-			clrsetbits_le32(&power_regs->hw_power_vddioctrl,
-				POWER_VDDIOCTRL_TRG_MASK, diff);
-
-			if (powered_by_linreg ||
-				(readl(&power_regs->hw_power_sts) &
-					POWER_STS_VDD5V_GT_VDDIO))
-				early_delay(500);
-			else {
-				while (!(readl(&power_regs->hw_power_sts) &
-					POWER_STS_DC_OK))
-					;
-
-			}
-
-			cur_target = readl(&power_regs->hw_power_vddioctrl);
-			cur_target &= POWER_VDDIOCTRL_TRG_MASK;
-			cur_target *= 50;	/* 50 mV step*/
-			cur_target += 2800;	/* 2800 mV lowest */
-		} while (new_target > cur_target);
-
-		if (powered_by_linreg) {
-			writel(POWER_CTRL_VDDIO_BO_IRQ,
-				&power_regs->hw_power_ctrl_clr);
-			if (bo_int & POWER_CTRL_ENIRQ_VDDIO_BO)
-				setbits_le32(&power_regs->hw_power_vddioctrl,
-						POWER_CTRL_ENIRQ_VDDIO_BO);
-		}
-	} else {
-		do {
-			if (cur_target - new_target > 100)
-				diff = cur_target - 100;
-			else
-				diff = new_target;
-
-			diff -= 2800;
-			diff /= 50;
-
-			clrsetbits_le32(&power_regs->hw_power_vddioctrl,
-				POWER_VDDIOCTRL_TRG_MASK, diff);
-
-			if (powered_by_linreg ||
-				(readl(&power_regs->hw_power_sts) &
-					POWER_STS_VDD5V_GT_VDDIO))
-				early_delay(500);
-			else {
-				while (!(readl(&power_regs->hw_power_sts) &
-					POWER_STS_DC_OK))
-					;
-
-			}
-
-			cur_target = readl(&power_regs->hw_power_vddioctrl);
-			cur_target &= POWER_VDDIOCTRL_TRG_MASK;
-			cur_target *= 50;	/* 50 mV step*/
-			cur_target += 2800;	/* 2800 mV lowest */
-		} while (new_target < cur_target);
+		setbits_le32(cfg->reg, cfg->bo_offset_mask);
 	}
 
-	clrsetbits_le32(&power_regs->hw_power_vddioctrl,
-			POWER_VDDIOCTRL_BO_OFFSET_MASK,
-			new_brownout << POWER_VDDIOCTRL_BO_OFFSET_OFFSET);
-}
-
-void mxs_power_set_vddd(uint32_t new_target, uint32_t new_brownout)
-{
-	struct mxs_power_regs *power_regs =
-		(struct mxs_power_regs *)MXS_POWER_BASE;
-	uint32_t cur_target, diff, bo_int = 0;
-	uint32_t powered_by_linreg = 0;
-
-	new_brownout = (new_target - new_brownout + 12) / 25;
-
-	cur_target = readl(&power_regs->hw_power_vdddctrl);
-	cur_target &= POWER_VDDDCTRL_TRG_MASK;
-	cur_target *= 25;	/* 25 mV step*/
-	cur_target += 800;	/* 800 mV lowest */
-
-	powered_by_linreg = mxs_get_vddd_power_source_off();
-	if (new_target > cur_target) {
-		if (powered_by_linreg) {
-			bo_int = readl(&power_regs->hw_power_vdddctrl);
-			clrbits_le32(&power_regs->hw_power_vdddctrl,
-					POWER_CTRL_ENIRQ_VDDD_BO);
-		}
-
-		setbits_le32(&power_regs->hw_power_vdddctrl,
-				POWER_VDDDCTRL_BO_OFFSET_MASK);
-
-		do {
-			if (new_target - cur_target > 100)
+	do {
+		if (abs(new_target - cur_target) > 100) {
+			if (adjust_up)
 				diff = cur_target + 100;
 			else
-				diff = new_target;
-
-			diff -= 800;
-			diff /= 25;
-
-			clrsetbits_le32(&power_regs->hw_power_vdddctrl,
-				POWER_VDDDCTRL_TRG_MASK, diff);
-
-			if (powered_by_linreg ||
-				(readl(&power_regs->hw_power_sts) &
-					POWER_STS_VDD5V_GT_VDDIO))
-				early_delay(500);
-			else {
-				while (!(readl(&power_regs->hw_power_sts) &
-					POWER_STS_DC_OK))
-					;
-
-			}
-
-			cur_target = readl(&power_regs->hw_power_vdddctrl);
-			cur_target &= POWER_VDDDCTRL_TRG_MASK;
-			cur_target *= 25;	/* 25 mV step*/
-			cur_target += 800;	/* 800 mV lowest */
-		} while (new_target > cur_target);
-
-		if (powered_by_linreg) {
-			writel(POWER_CTRL_VDDD_BO_IRQ,
-				&power_regs->hw_power_ctrl_clr);
-			if (bo_int & POWER_CTRL_ENIRQ_VDDD_BO)
-				setbits_le32(&power_regs->hw_power_vdddctrl,
-						POWER_CTRL_ENIRQ_VDDD_BO);
-		}
-	} else {
-		do {
-			if (cur_target - new_target > 100)
 				diff = cur_target - 100;
-			else
-				diff = new_target;
+		} else {
+			diff = new_target;
+		}
 
-			diff -= 800;
-			diff /= 25;
+		diff -= cfg->lowest_mV;
+		diff /= cfg->step_mV;
 
-			clrsetbits_le32(&power_regs->hw_power_vdddctrl,
-					POWER_VDDDCTRL_TRG_MASK, diff);
+		clrsetbits_le32(cfg->reg, cfg->trg_mask, diff);
 
-			if (powered_by_linreg ||
-				(readl(&power_regs->hw_power_sts) &
-					POWER_STS_VDD5V_GT_VDDIO))
-				early_delay(500);
-			else {
-				while (!(readl(&power_regs->hw_power_sts) &
-					POWER_STS_DC_OK))
-					;
-
+		if (powered_by_linreg ||
+			(readl(&power_regs->hw_power_sts) &
+				POWER_STS_VDD5V_GT_VDDIO))
+			early_delay(500);
+		else {
+			for (;;) {
+				tmp = readl(&power_regs->hw_power_sts);
+				if (tmp & POWER_STS_DC_OK)
+					break;
 			}
+		}
 
-			cur_target = readl(&power_regs->hw_power_vdddctrl);
-			cur_target &= POWER_VDDDCTRL_TRG_MASK;
-			cur_target *= 25;	/* 25 mV step*/
-			cur_target += 800;	/* 800 mV lowest */
-		} while (new_target < cur_target);
+		cur_target = readl(cfg->reg);
+		cur_target &= cfg->trg_mask;
+		cur_target *= cfg->step_mV;
+		cur_target += cfg->lowest_mV;
+	} while (new_target > cur_target);
+
+	if (adjust_up && powered_by_linreg) {
+		writel(cfg->bo_irq, &power_regs->hw_power_ctrl_clr);
+		if (bo_int & cfg->bo_enirq)
+			setbits_le32(cfg->reg, cfg->bo_enirq);
 	}
 
-	clrsetbits_le32(&power_regs->hw_power_vdddctrl,
-			POWER_VDDDCTRL_BO_OFFSET_MASK,
-			new_brownout << POWER_VDDDCTRL_BO_OFFSET_OFFSET);
+	clrsetbits_le32(cfg->reg, cfg->bo_offset_mask,
+			new_brownout << cfg->bo_offset_offset);
 }
 
 void mxs_setup_batt_detect(void)
@@ -982,9 +896,8 @@
 	mxs_power_configure_power_source();
 	mxs_enable_output_rail_protection();
 
-	mxs_power_set_vddio(3300, 3150);
-
-	mxs_power_set_vddd(1350, 1200);
+	mxs_power_set_vddx(&mxs_vddio_cfg, 3300, 3150);
+	mxs_power_set_vddx(&mxs_vddd_cfg, 1350, 1200);
 
 	writel(POWER_CTRL_VDDD_BO_IRQ | POWER_CTRL_VDDA_BO_IRQ |
 		POWER_CTRL_VDDIO_BO_IRQ | POWER_CTRL_VDD5V_DROOP_IRQ |